#include "kernel/feMvOps.h"
#include "conductivelayer.h"
#include "kernel/feLuSolver.h"
#include "FastMath.h"
#include "ReverseTask.h"
#include <stdio.h>
#include <time.h>
#include "kernel/fePolylineGenerator.h"

ReverseTask::ReverseTask() : SlaeRight(RightBuf)
{
	//init layers
	for(int i=0; i<MAX_LAYERS; i++)
	{
		Medium[i].H = 0;
		Medium[i].Sigma = 0;
	}

	DerMatrixTransposed = new double *[MAX_ROWS];
	for(int i=0; i<MAX_ROWS; i++)
		DerMatrixTransposed[i] = new double[MAX_COLS];
	RegCoeffSigma = RegCoeffH = 0;
}


//---------------------------------------------------------------------------------------------------
void ReverseTask::LogMedium(ConductiveLayer *iMedium, ConductiveLayer *oMedium, int iLayersCount)
{
	for(int i=0; i<iLayersCount; i++)
	{
		oMedium[i].H = log10(iMedium[i].H);
		oMedium[i].Sigma = log10(iMedium[i].Sigma);
	}
}


//---------------------------------------------------------------------------------------------------
void ReverseTask::ExpMedium(ConductiveLayer *iMedium, ConductiveLayer *oMedium, int iLayersCount)
{
	for(int i=0; i<iLayersCount; i++)
	{
		oMedium[i].H = pow(10.0, iMedium[i].H);
		oMedium[i].Sigma = pow(10.0, iMedium[i].Sigma);
	}
}


//---------------------------------------------------------------------------------------------------
void ReverseTask::BindData(ConductiveLayer *iLayers, int iLayersCount, Curve *iOrigCurves, int iCurvesCount, double iRegCoeffH, double iRegCoeffSigma)
{
	//copy layers
	for(int i=0; i<iLayersCount; i++)
	{
		OldMedium[i].H = Medium[i].H = iLayers[i].H;
		OldMedium[i].Sigma = Medium[i].Sigma = iLayers[i].Sigma;		
	}
	LayersNum = iLayersCount;
	LogMedium(OldMedium, OldMediumLog, LayersNum);
	for(int i=0; i<iLayersCount; i++)
	{
		MediumLog[i] = OldMediumLog[i];
		FirstMediumLog[i] = OldMediumLog[i];
	}
		
	RegCoeffSigma = iRegCoeffSigma;
	RegCoeffH = iRegCoeffH;

	//copy curve
	CurvesCount = iCurvesCount;
	for(int c=0; c<iCurvesCount; c++)
	{
		OriginalCurves[c].CopyFrom(iOrigCurves[c]);
		OriginalCurves[c].Filter();
		AbnomalCurves[c].Init(0, iOrigCurves[c].GetSize());
		ActualCurves[c].Init(0, OriginalCurves[c].GetSize());
		for(int i=0; i<OriginalCurves[c].GetSize(); i++)
		{
			ActualCurves[c][i].X = OriginalCurves[c][i].X;
			AbnomalCurves[c][i].X = OriginalCurves[c][i].X;
		}
	}

	LastSSqr = 0;
	
	AfterInit();
}


//---------------------------------------------------------------------------------------------------
void ReverseTask::FillE(Curve *iOrigCurve, Curve *iActCurve, feVector *oE)
{
	for(int i=0; i<iActCurve->GetSize(); i++)
	{
		oE->Data[i] = (iActCurve->GetPoint(i).Y - iOrigCurve->GetPoint(i).Y)/(iOrigCurve->GetPoint(i).Y);
	}
}


//---------------------------------------------------------------------------------------------------
void ReverseTask::GetE0()
{
	double s = 0;
	for(int i=0; i<CurvesCount; i++)
	{
		E0[i].Allocate(OriginalCurves[i].GetSize());
		E[i].Allocate(OriginalCurves[i].GetSize());
		
		//get e0 = (ActualCurve - OriginalCurve) / OriginalCurve
		CalcActualCurve(i);
		FillE(&ActualCurves[i], &OriginalCurves[i], &E0[i]);
		s += feMvOps::DotVV(&E0[i], &E0[i]);

		FillE(&OriginalCurves[i], &ActualCurves[i], &E0[i]);
		s += feMvOps::DotVV(&E0[i], &E0[i]);
		

		FillE(&OriginalCurves[i], &ActualCurves[i], &E[i]);	
	}

	if(LastSSqr == 0) LastSSqr += s/2;
}


//---------------------------------------------------------------------------------------------------
void ReverseTask::FillDerivMatrix()
{	
	for(int j=0; j<LayersNum; j++)
		MediumLog[j] = OldMediumLog[j];


	//fill deriv matrix
	double delta = 1e-2;
	

	//zero slae right
	DerMatrixRows = 2*LayersNum - 1;
	DerMatrixCols = OriginalCurves[0].GetSize();	

	SlaeRight.Allocate(DerMatrixRows);
	for(int i=0; i<DerMatrixRows; i++)
		SlaeRight[i] = 0;


	
	//allocate slae matrix
	SlaeMatrix.Allocate(DerMatrixRows);
	RegSlaeMatrix.Allocate(DerMatrixRows);
	SlaeX0.Allocate(DerMatrixRows);
	SlaeX1.Allocate(DerMatrixRows);


	//zero slae maitrx
	for(int i=0; i<DerMatrixRows; i++)
	{
		for(int j=0; j<DerMatrixRows; j++)
		{
			double *ij0 = SlaeMatrix._IJ(i, j);			
			(*ij0) = 0;
		}
	}


	for(int c=0; c<CurvesCount; c++)	
	{
		for(int i=0; i<LayersNum; i++)
		{
			delta = 0.001;
			MediumLog[i].Sigma = OldMediumLog[i].Sigma + delta;			
			
			ExpMedium(MediumLog, Medium, LayersNum);
			CalcAbnormalCurve(c, Medium, LayersNum);

			//fill the matrix
			for(int j=0; j<AbnomalCurves[c].GetSize(); j++)
				DerMatrixTransposed[2*i][j] = AbnomalCurves[c][j].Y / delta / OriginalCurves[c][j].Y;
			

			MediumLog[i].Sigma = OldMediumLog[i].Sigma;

			if(i != LayersNum - 1)
			{
				//h-step			
				delta = 0.001;
				MediumLog[i].H = OldMediumLog[i].H + delta;
				ExpMedium(MediumLog, Medium, LayersNum);
				CalcAbnormalCurve(c, Medium, LayersNum);

				//fill the matrix
				for(int j=0; j<AbnomalCurves[c].GetSize(); j++)
					DerMatrixTransposed[2*i+1][j] = AbnomalCurves[c][j].Y / delta / OriginalCurves[c][j].Y;


				MediumLog[i].H = OldMediumLog[i].H;
			}
		}


		//fill slae matrix
		for(int i=0; i<DerMatrixRows; i++)
		{
			for(int j=0; j<DerMatrixRows; j++)
			{
				double *ij0 = SlaeMatrix._IJ(i, j);			
				for(int k=0; k<DerMatrixCols; k++)
					(*ij0) += DerMatrixTransposed[i][k]*DerMatrixTransposed[j][k];
			}
		}
		for(int i=0; i<DerMatrixRows; i++)
		{
			for(int j=0; j<DerMatrixCols; j++)
			{
				SlaeRight[i] -= DerMatrixTransposed[i][j] * E0[c][j];
			}
		}
	}

	//apply reg coeffs
	for(int i=0; i<LayersNum; i++)
	{
		SlaeRight[2*i] += RegCoeffSigma * (FirstMediumLog[i].Sigma - OldMediumLog[i].Sigma);
		if(i!=LayersNum-1) 
		{
			SlaeRight[2*i+1] += RegCoeffH * (FirstMediumLog[i].H - OldMediumLog[i].H);
		}
	}
}


//---------------------------------------------------------------------------------------------------
void ReverseTask::GetU(double iAlpha, feVector *oX)
{
	for(int i=0; i<SlaeMatrix.GetSize(); i++)
	{
		for(int j=0; j<SlaeMatrix.GetSize(); j++)
		{
			(*RegSlaeMatrix._IJ(i, j)) = (*SlaeMatrix._IJ(i, j)) + iAlpha * (i == j ? 1 : 0);
		}
	}

	for(int i=0; i<LayersNum; i++)
	{
		double *ij = RegSlaeMatrix._IJ(2*i, 2*i);			
		(*ij) += RegCoeffSigma;
		if(i!=LayersNum-1) 
		{
			double *ij = RegSlaeMatrix._IJ(2*i+1, 2*i+1);			
			(*ij) += RegCoeffH;
		}
	}

	Solver.BindData(&RegSlaeMatrix, &SlaeRight, oX);
	Solver.Iterate();
	Solver.GetX(oX);
}


//---------------------------------------------------------------------------------------------------
void ReverseTask::FillU()
{
	feVector *u = &SlaeX0;
	feVector *u_prev = &SlaeX1;

	double alpha = 1e-10;
	double alpha_prev = -1;

	double startdegree = -10;
	double pointsperdegree = 10;

	int pointscount = 200;
	DuDalpha1.Allocate(pointscount);
	DuDalpha2.Allocate(pointscount);
	DuDalpha1.Clear();
	DuDalpha2.Clear();

	double normu = 0;
	double normu_prev = 0;
	//fill u
	for(int i=0; i<pointscount; i++)
	{	
		alpha = pow(10, startdegree + i/pointsperdegree);
		
		GetU(alpha, u);
		normu = FastMath::Sqrt(feMvOps::DotVV(u, u));

		if(alpha_prev != -1)
		{
			//fill derivative
			double norm = 0;
			for(int k=0; k<u_prev->GetSize(); k++)
			{
				double d = (u->Data[k] - u_prev->Data[k]);
				norm += d*d;
			}
			DuDalpha1.Push(alpha, alpha*fabs(norm)/(alpha - alpha_prev));
			DuDalpha2.Push(alpha, alpha*(normu-normu_prev)/(alpha - alpha_prev));
		}
			
		feVector *b = u;
		u = u_prev;
		u_prev = b;
		
		normu_prev = normu;
		alpha_prev = alpha;
	}
}


//---------------------------------------------------------------------------------------------------
bool ReverseTask::PerformStep()
{
	for(int i=0; i<LayersNum; i++)
	{
		MediumLog[i].Sigma = OldMediumLog[i].Sigma + SlaeX1[2*i];
		if(i != LayersNum - 1)
			MediumLog[i].H = OldMediumLog[i].H + SlaeX1[2*i+1];
	}
	return true;
}


//---------------------------------------------------------------------------------------------------
double ReverseTask::GetNorm(double iT)
{
	for(int i=0; i<LayersNum; i++)
	{
		SlaeX1[2*i] = iT * SlaeX0[2*i];
		if(i != LayersNum-1)
			SlaeX1[2*i+1] = iT * SlaeX0[2*i+1];
	}

	PerformStep();

	double res = 0;
	for(int c=0; c<CurvesCount; c++)
	{
		ExpMedium(MediumLog, Medium, LayersNum);
		CalcActualCurve(c);
		double s = 0;
		FillE(&ActualCurves[c], &OriginalCurves[c], &E[c]);

		//get norm
		s += feMvOps::DotVV(&E[c], &E[c]);

		FillE(&OriginalCurves[c], &ActualCurves[c], &E[c]);

		//get norm
		//s += feMvOps::DotVV(&E[c], &E[c]);
		res += s/2;
	}

	//get norm
	return res;
}


//---------------------------------------------------------------------------------------------------
void ReverseTask::GetOptStep()
{
	StepFound = false;
	//get minimums of dudalpha
	double minimums[1000];
	double norms[1000];
	double norms1[1000];
	int mincount = 0;
	for(int i=2; i<DuDalpha1.GetSize(); i++)
	{
		if(DuDalpha1.GetPoint(i).Y  > DuDalpha1.GetPoint(i-1).Y && DuDalpha1.GetPoint(i-1).Y < DuDalpha1.GetPoint(i-2).Y)
		{
			minimums[mincount] = DuDalpha1.GetPoint(i-1).X;
			mincount++;
		}
//  		minimums[mincount] = DuDalpha1.GetPoint(i-1).X;
// 		norms[mincount] = DuDalpha1.GetPoint(i-1).Y;
// 		GetU(minimums[mincount], &SlaeX0);
// 		norms1[mincount] = GetNorm(1.0) / LastSSqr;
// 		mincount++;
	}
// 	//fePolylineGenerator g;
//  	//g.SavePolylineToFile(&g.CreatePolylineFromArray(minimums, norms, mincount, 10), "DuDalpha");
//  	fePolylineGenerator gg;
//  	gg.SavePolylineToFile(&gg.CreatePolylineFromArray(minimums, norms1, mincount, 10), "S");


	double best_alpha = 0;

	
	for(int i=mincount-1; i>=0; i--)
	{
		GetU(minimums[i], &SlaeX1);

		if(PerformStep())
		{
			ExpMedium(MediumLog, Medium, LayersNum);
			//calc E
			norms[i] = 0;
			for(int c=0; c<CurvesCount; c++)
			{
				CalcActualCurve(c);
				double s = 0;
				FillE(&ActualCurves[c], &OriginalCurves[c], &E[c]);					
				s += feMvOps::DotVV(&E[c], &E[c]);

				FillE(&OriginalCurves[c], &ActualCurves[c], &E[c]);					
				s += feMvOps::DotVV(&E[c], &E[c]);
				norms[i] += s/2;
			}

			if(norms[i] < LastSSqr)
			{
				LastSSqr = norms[i];
				StepFound = true;
				best_alpha = minimums[i];
				break;
			}
		}
	}
	


	//not found right step - perform 1d search
	if(StepFound == true)
	{
		GetU(best_alpha, &SlaeX1);
	}
	else
	{
		//try second dudalpha
		mincount = 0;
		for(int i=2; i<DuDalpha2.GetSize(); i++)
		{
			if(DuDalpha2.GetPoint(i).Y  > DuDalpha2.GetPoint(i-1).Y && DuDalpha2.GetPoint(i-1).Y < DuDalpha2.GetPoint(i-2).Y)
			{
				minimums[mincount] = DuDalpha2.GetPoint(i-1).X;
				mincount++;
			}
		}


		for(int i=mincount-1; i>=0; i--)
		{
			GetU(minimums[i], &SlaeX1);

			if(PerformStep())
			{
				ExpMedium(MediumLog, Medium, LayersNum);
				norms[i] = 0;
				for(int c=0; c<CurvesCount; c++)
				{
					CalcActualCurve(c);

					double s = 0;
					FillE(&ActualCurves[c], &OriginalCurves[c], &E[c]);					
					s += feMvOps::DotVV(&E[c], &E[c]);

					FillE(&OriginalCurves[c], &ActualCurves[c], &E[c]);					
					s += feMvOps::DotVV(&E[c], &E[c]);
					norms[i] += s/2;
				}

				if(norms[i] < LastSSqr)
				{
					LastSSqr = norms[i];
					StepFound = true;
					best_alpha = minimums[i];
					break;
				}
			}
		}
		

		if(StepFound == true)
		{
			GetU(best_alpha, &SlaeX1);
		}
		else
		{
			if(mincount == 0)
			{
				minimums[0] = 10;
				mincount = 1;
			}

			const double phi = (FastMath::Sqrt(5.0) - 1)/2;
			double res_t = 0;
			for(int i=mincount-1; i>=0; i--)
			{
				GetU(minimums[i], &SlaeX0);

				double mint = 0;
				double maxt = 1;

				
				double res_n = 0;
				int iters = 0;

				double t1 = maxt - (maxt-mint)*phi;
				double norm1 = GetNorm(t1);
				//check ending condition
				if(norm1 < LastSSqr)
				{
					res_t = t1;
					res_n = norm1;
				}


				double t2 = mint + (maxt-mint)*phi;
				double norm2 = GetNorm(t2);
				
				//check ending condition
				if(norm2 < LastSSqr)
				{
					res_t = t2;
					res_n = norm2;
				}

				while(res_n == 0 && abs(maxt-mint) > 1e-3 /*&& iters < 5*/)
				{
					if(norm1 <= norm2)
					{
						maxt = t2;
						t2 = t1;
						norm2 = norm1;

						t1 = maxt - (maxt-mint)*phi;	
						norm1 = GetNorm(t1);
					}
					else
					{
						mint = t1;
						t1 = t2;
						norm1 = norm2;

						t2 = mint + (maxt-mint)*phi;					
						norm2 = GetNorm(t2);
					}
					iters++;

					//check ending condition
					if(norm1 < LastSSqr)
					{
						res_t = t1;
						res_n = norm1;
						break;
					}

					//check ending condition
					if(norm2 < LastSSqr)
					{
						res_t = t2;
						res_n = norm2;
						break;
					}
				}

				//found good step
				if(res_t != 0)
				{
					//remember it and finish
					LastSSqr = res_n;
					StepFound = true;
					for(int i=0; i<LayersNum; i++)
					{
						SlaeX1[2*i] = res_t * SlaeX0[2*i];
						if(i != LayersNum-1)
							SlaeX1[2*i+1] = res_t * SlaeX0[2*i+1];
					}
					break;
				}
			}			
		}
	}
}


//---------------------------------------------------------------------------------------------------
void ReverseTask::Iterate()
{	
	GetE0();
	FillDerivMatrix();
	FillU();
	GetOptStep();
	if(StepFound)
	{
		PerformStep();	
		
		//save medium
		ExpMedium(MediumLog, Medium, LayersNum);
		for(int i=0; i<LayersNum; i++)
		{
			OldMedium[i] = Medium[i];	
			OldMediumLog[i] = MediumLog[i];
		}
	}
	else
	{
		for(int i=0; i<LayersNum; i++)
		{
			Medium[i] = OldMedium[i];	
			MediumLog[i] = OldMediumLog[i];
		}
	}

}


//---------------------------------------------------------------------------------------------------
double ReverseTask::GetEps()
{
	return FastMath::Sqrt(LastSSqr);
}


//---------------------------------------------------------------------------------------------------
void ReverseTask::GetSolvation(ConductiveLayer *oLayers, int &oLayersNum)
{
	oLayersNum = LayersNum;
	for(int i=0; i<LayersNum; i++)
	{
		oLayers[i].H = Medium[i].H;
		oLayers[i].Sigma = Medium[i].Sigma;
	}
}


//---------------------------------------------------------------------------------------------------
void ReverseTask::SplitMesh()
{
	double maxnorm = 0;
	int maxlayer = 0;


	//try to merge mesh
	int merged_num = 0;
	ExpMedium(MediumLog, Medium, LayersNum);
	for(int i=1; i<LayersNum-merged_num; i++)
	{
		if(abs(Medium[i].Sigma - Medium[i-1].Sigma) < 1e-2 * abs(max(Medium[i].Sigma, Medium[i-1].Sigma))) 
		{
			Medium[i-1].Sigma = Medium[i].Sigma;
			Medium[i-1].Sigma /= 2;
			Medium[i-1].H += Medium[i].H;
			//shift layers
			for(int j=i; j<LayersNum-1; j++)
			{
				Medium[j] = Medium[j+1];
			}
			merged_num++;
			i--;
		}
	}
	LayersNum -= merged_num;
	Medium[LayersNum-1].H = 1e20;
	LogMedium(Medium, MediumLog, LayersNum);
	
	
	//remove too thin layers
	for(int i=0; i<LayersNum; i++)
	{
		//remove layer
		if(abs(MediumLog[i].H) <1e-4)
		{
			for(int j=i+1; j<LayersNum; j++)
				MediumLog[j-1] = MediumLog[j];
			LayersNum--;
		}
	}
	MediumLog[LayersNum-1].H = 20;


	//try to increase each sigma	
	double s = 0;
	double maxs = 0;
	for(int i=0; i<LayersNum; i++)
	{
		double old_sigma = Medium[i].Sigma;
		Medium[i].Sigma *= 10;
		
		//get norm
		double n = 0;
		for(int c=0; c<CurvesCount; c++)
		{
			CalcAbnormalCurve(c, Medium, LayersNum);
			for(int j=0; j<E0[c].GetSize(); j++)
			{
				E[c][j] = AbnomalCurves[c][j].Y/OriginalCurves[c][j].Y;
			}

			//get norm
			n += feMvOps::DotVV(&E[c], &E[c]);
		}


		Medium[i].Sigma = old_sigma;
		
		if(i != LayersNum -1)
			s += Medium[i].Sigma*Medium[i].H;
		
		if(n > maxnorm) 
		{
			maxs = s;
			maxnorm = n;
			maxlayer = i;
		}
	}


	//split layer with max deviation
	for(int i=LayersNum-1; i>=maxlayer; i--)
		MediumLog[i+1] = MediumLog[i];

	if(MediumLog[maxlayer].H < 10)
	{
		MediumLog[maxlayer+1].H = MediumLog[maxlayer].H - log10(2.0);
		MediumLog[maxlayer].H =  MediumLog[maxlayer].H - log10(2.0);
	}
	else
	{
		if(LayersNum > 1)
		{
			MediumLog[maxlayer].H = log10(maxs*10+1);
		}
		else 
		{
			double old_sigma = MediumLog[0].Sigma;
			MediumLog[0].Sigma += 1;
			double maxt = 0;
			double maxn = 0;
			double h = 0;
			

			for(int c=0; c<CurvesCount; c++)
			{
				ExpMedium(MediumLog, Medium, LayersNum);
				CalcActualCurve(c);
				FillE(&OriginalCurves[c], &ActualCurves[c], &E[c]);
				for(int i=1; i<E0[c].GetSize(); i++)
				{
					if(abs(E[c][i]) > maxn && OriginalCurves[c][i].X > 1e-4)
					{
						maxt = OriginalCurves[c][i].X;
						maxn = abs(E[c][i]);
					}
				}
			}
			h = log10(sqrt(maxt/(Medium[0].Sigma*MU0)));

			if(h > 3 || h < -1)
			{
				for(int c=0; c<CurvesCount; c++)
				{
					ExpMedium(MediumLog, Medium, LayersNum);
					CalcAbnormalCurve(c, Medium, LayersNum);
					for(int i=1; i<E0[c].GetSize(); i++)
					{
						if(AbnomalCurves[c][i].Y*AbnomalCurves[c][i-1].Y < 0 && maxt == 0)
						{
							maxt = OriginalCurves[c][i].X;
						}
					}
				}
				h = 4*log10(sqrt(maxt/(Medium[0].Sigma*MU0)));
			}
			if(h > 3 || h < -1) h = 1.5;					
			

			MediumLog[0].Sigma = old_sigma;

			ExpMedium(MediumLog, Medium, LayersNum);
			MediumLog[maxlayer].H = h;
			
		}

		MediumLog[maxlayer+1].H = 20;
	}
	LayersNum++;

	ExpMedium(MediumLog, Medium, LayersNum);
	for(int i=0; i<LayersNum; i++)
	{
		OldMedium[i] = Medium[i];
		OldMediumLog[i] = FirstMediumLog[i] = MediumLog[i];
	}
	
}


//---------------------------------------------------------------------------------------------------
ReverseTask::~ReverseTask()
{
	for(int i=0; i<MAX_ROWS; i++)
		delete [] DerMatrixTransposed[i];
	delete [] DerMatrixTransposed;
	DerMatrixTransposed = 0;
}